#
# Motor Driver L298N
#
# pigpio -hardware PMW
#
import RPi.GPIO as GPIO
import pigpio
import time

class Motor:

    GO = 0
    BACK = 1
    STOP = 2
    LEFT = 3
    RIGHT = 4

    LEFT_STEP = 5
    RIGHT_STEP = 6

    ADD_SPEED=10 
    MIN_SPEED=20
    
    SPEED_100=255
    SPEED_75=0.75*SPEED_100
    SPEED_50=0.5*SPEED_100
    SPEED_25=0.25*SPEED_100
    
    
    MAX_SPEED=SPEED_100 # 电机最大速度
    MIN_SPEED=SPEED_50 # 低于该速度,电机不能转动
    STEP_SPEED=10 # 速度加减步长

    MAX_TURN_MAIN_SPEED=SPEED_100 # 最大主转向轮速度
    MIN_TURN_MAIN_SPEED=SPEED_75 #  最低主转向轮速度
    MAX_DIFF_SPEED_PERCENT=100 # 左，右侧速度差,全速转向,最大速度差100% 一侧停止
     
    def __init__(self,IN1,IN2,IN3,IN4,ENA,ENB):
        self.Left_Red = IN2
        self.Left_Black =  IN1
       
        self.Right_Black = IN4
        self.Right_Red = IN3
        
        self.Left_Enable = ENA
        self.Right_Enable = ENB 
        
        self.MOTOR_STATE = Motor.STOP
        
        # 前进、后退速度
        self.cur_speed= Motor.MIN_SPEED # the default forward/backward speed 
        # 转向速度
        self.turn_main_speed=Motor.MAX_TURN_MAIN_SPEED #  主转动轮速度
        self.turn_diff_speed_percent=Motor.MAX_DIFF_SPEED_PERCENT
        
        # init
        GPIO.setup(self.Left_Black, GPIO.OUT)
        GPIO.setup(self.Left_Red, GPIO.OUT)
        GPIO.setup(self.Right_Black, GPIO.OUT)
        GPIO.setup(self.Right_Red, GPIO.OUT)
        
        # speed PWM
        self.Left_PMW=pigpio.pi()
        self.Right_PMW=pigpio.pi()

        self.Left_PMW.set_PWM_range(self.Left_Enable,  Motor.SPEED_100)
        self.Right_PMW.set_PWM_range(self.Right_Enable, Motor.SPEED_100)
         
        self.Left_PMW.set_PWM_dutycycle(self.Left_Enable,0) 
        self.Right_PMW.set_PWM_dutycycle(self.Right_Enable,0) 
        
        self.DICT_CMD_ACTION = {Motor.GO: self.go,
                                Motor.BACK: self.back,
                                Motor.STOP: self.stop,
                                Motor.LEFT: self.turn_left,
                                Motor.RIGHT: self.turn_right,
                                Motor.LEFT_STEP: self.turn_left_step,
                                Motor.RIGHT_STEP: self.turn_right_step
                                }

    def go(self):
        self.Left_PMW.set_PWM_dutycycle(self.Left_Enable,self.cur_speed) 
        GPIO.output(self.Left_Red, GPIO.HIGH)
        GPIO.output(self.Left_Black, GPIO.LOW)
        # 
        self.Right_PMW.set_PWM_dutycycle(self.Right_Enable,self.cur_speed) 
        GPIO.output(self.Right_Red, GPIO.HIGH)
        GPIO.output(self.Right_Black, GPIO.LOW)

    def back(self):
        self.Left_PMW.set_PWM_dutycycle(self.Left_Enable,self.cur_speed) 
        GPIO.output(self.Left_Red, GPIO.LOW)
        GPIO.output(self.Left_Black, GPIO.HIGH)
        self.Right_PMW.set_PWM_dutycycle(self.Right_Enable,self.cur_speed) 
        GPIO.output(self.Right_Red, GPIO.LOW)
        GPIO.output(self.Right_Black, GPIO.HIGH)

    def stop(self):
        self.Left_PMW.set_PWM_dutycycle(self.Left_Enable,0) 
        GPIO.output(self.Left_Red, GPIO.LOW)
        GPIO.output(self.Left_Black, GPIO.LOW)
        self.Right_PMW.set_PWM_dutycycle(self.Right_Enable,0) 
        GPIO.output(self.Right_Red, GPIO.LOW)
        GPIO.output(self.Right_Black, GPIO.LOW)

    def turn_left(self):
        # 主转向轮
        if self.cur_speed>Motor.MIN_TURN_MAIN_SPEED:
           self.turn_main_speed=self.cur_speed
        else:
           self.turn_main_speed=Motor.MIN_TURN_MAIN_SPEED     
        self.Left_PMW.set_PWM_dutycycle(self.Right_Enable,self.turn_main_speed) 
        GPIO.output(self.Right_Red, GPIO.HIGH)
        GPIO.output(self.Right_Black, GPIO.LOW)
        # 低速轮
        lower_speed=int(self.turn_main_speed*(1.0-self.turn_diff_speed_percent/100.0))
        self.Left_PMW.set_PWM_dutycycle(self.Left_Enable,lower_speed) 
        GPIO.output(self.Left_Red, True)
        GPIO.output(self.Left_Black, False)

    def turn_left_step(self):
        self.turn_left()
        time.sleep(0.3)
        self.stop()
        self.MOTOR_STATE= Motor.STOP

    def turn_right(self):
        # 主转向轮
        if self.cur_speed>Motor.MIN_TURN_MAIN_SPEED:
           self.turn_main_speed=self.cur_speed
        else:
           self.turn_main_speed=Motor.MIN_TURN_MAIN_SPEED     
        self.Left_PMW.set_PWM_dutycycle(self.Left_Enable,self.turn_main_speed) 
        GPIO.output(self.Left_Red, GPIO.HIGH)
        GPIO.output(self.Left_Black, GPIO.LOW)
        # 低速轮
        lower_speed=int(self.turn_main_speed*(1.0-self.turn_diff_speed_percent/100.0))
        self.Right_PMW.set_PWM_dutycycle(self.Right_Enable,lower_speed) 
        GPIO.output(self.Right_Red, True)
        GPIO.output(self.Right_Black, False)
   
    def turn_right_step(self):
        self.turn_right()
        time.sleep(0.3)
        self.stop()
        self.MOTOR_STATE= Motor.STOP

    def action(self, cmd_to_motor):
        if (cmd_to_motor != self.MOTOR_STATE):
            self.MOTOR_STATE = cmd_to_motor
            self.DICT_CMD_ACTION[cmd_to_motor]()
    
    def adjust_speed(self,add_min_speed):
        # 加、减速
        if (add_min_speed==Motor.ADD_SPEED):
            self.cur_speed=self.cur_speed+Motor.STEP_SPEED
            if self.cur_speed>Motor.MAX_SPEED:
                self.cur_speed=Motor.MAX_SPEED
        elif (add_min_speed==Motor.MIN_SPEED):
            self.cur_speed=self.cur_speed-Motor.STEP_SPEED
            if self.cur_speed<Motor.MIN_SPEED:
                self.cur_speed=Motor.MIN_SPEED
        self.DICT_CMD_ACTION[self.MOTOR_STATE]()


    def adjust_turn_speed(self, set_turn_diff_speed_percent):
        # 转向速度 -控制2侧速度差
        if (set_turn_diff_speed_percent!=self.turn_diff_speed_percent):
            self.turn_diff_speed_percent=set_turn_diff_speed_percent
            if (self.MOTOR_STATE== Motor.LEFT):
               self.turn_left()
            elif (self.MOTOR_STATE== Motor.RIGHT): 
               self.turn_right()